/*
 * RT MAP, Home of Professional MAP
 * Copyright 2021 Bit Main Inc. and/or its affiliates and other contributors
 * as indicated by the @author tags. All rights reserved.
 * See the copyright.txt in the distribution for a
 * full listing of individual contributors.
 */
package com.example.demo.service;



import com.example.demo.model.Path;
import com.example.demo.model.Point;
import com.example.demo.model.Vertex;

import java.util.*;

/**
 * @author CaoYuyao
 * @title NaviTool
 * @description 计算路径
 * @date 2021/9/10
 */
public class NaviTool {
    private ArrayList<Path> paths;
    private ArrayList<Point> points;
    private HashMap<String, List<Vertex>> vertices;
    /**
     * 记录路线宽度，用于偏移
     */
    private HashMap<String, Double> offsetMap;
    private HashMap<String, String> cameraMap;
    public NaviTool(ArrayList<Point> points, ArrayList<Path> paths) {
        this.paths = paths;
        this.points = points;
        this.vertices = this.initData(this.points, this.paths);
        this.offsetMap = this.initOffset(this.paths);
        this.cameraMap=this.initCamera(this.points);
    }

    private HashMap<String, String>initCamera(ArrayList<Point> points){
        HashMap<String, String> cameraMap = new HashMap<String, String>();
        for (Point point : points) {
            if(point.getCamera()!=null&& !"".equals(point.getCamera())){
                cameraMap.put(point.getCamera(),point.getId());
            }
        }

        return cameraMap;
    }
    /**
     * 初始化offsetMap数据
     *
     * @param paths 路线数据
     * @return offsetMap数据
     */
    private HashMap<String, Double> initOffset(ArrayList<Path> paths) {
        HashMap<String, Double> offsetMap = new HashMap<>();
        for (Path path : paths) {
            offsetMap.put(path.getStart() + "_" + path.getEnd(), path.getWidth());
            offsetMap.put(path.getEnd() + "_" + path.getStart(), path.getWidth());
        }
        return offsetMap;
    }
    /**
     * 路径规划
     *
     * @param cameraStart 起点摄像头
     * @param cameraEnd   终点摄像头
     * @return 结果
     * @throws Exception
     */
    public List<Point> navigateByCamera(String cameraStart, String cameraEnd) throws Exception {
        String start=this.cameraMap.get(cameraStart);
        if(start==null|| "".equals(start)){
            throw new Exception("未找到起点摄像头");
        }

        String end=this.cameraMap.get(cameraEnd);
        if(end==null|| "".equals(end)){
            throw new Exception("未找到终点摄像头");
        }

        return this.navigate(start,end);
    }
        /**
         * 路径规划
         *
         * @param start 起点id
         * @param end   终点id
         * @return 结果
         * @throws Exception
         */
    public List<Point> navigate(String start, String end) throws Exception {
        if (this.paths == null || this.points == null || this.vertices == null || this.offsetMap == null) {
            throw new Exception("未初始化");
        }
        if (start.equals(end)) {
            throw new Exception("起点终点不能一样");
        }
        List<String> shortestPath = this.getShortestPath(start, end);
        if (shortestPath == null || shortestPath.size() < 2) {
            throw new Exception("路径规划失败");
        }
        List<Point> result = new ArrayList<>();
        double width = 0;
        for (int i = 0; i < shortestPath.size(); i++) {
            String id = shortestPath.get(i);
            for (Point point : this.points) {
                if (point.getId().equals(id)) {
                    if (i == 0) {
                        String idEnd = shortestPath.get(i + 1);
                        width = this.getOffsetWidth(id, idEnd);

                    } else if (i == shortestPath.size() - 1) {
                        String idStart = shortestPath.get(i - 1);
                        width = this.getOffsetWidth(idStart, id);

                    } else {
                        String idPrev = shortestPath.get(i - 1);
                        String idNext = shortestPath.get(i + 1);
                        double widthPrev = this.getOffsetWidth(idPrev, id);
                        double widthNext = this.getOffsetWidth(id, idNext);
                        width = Math.min(widthPrev, widthNext);
                    }
                    Point point1=point.clone();
                    point1.setRandomOffset(width);
                    result.add(point1);
                }
            }
        }
        return result;
    }

    /**
     * 处理数据，将点和线制作成路网
     *
     * @param points 点数据
     * @param paths  线数据
     * @return 路网数据
     */
    public HashMap<String, List<Vertex>> initData(ArrayList<Point> points, ArrayList<Path> paths) {
        //解析数据，确定点和周围的点的关系
        HashMap<String, List<Vertex>> vertexMap = new HashMap<String, List<Vertex>>();
        for (Point point : points) {
            //计算和point相连的path
            List<Vertex> neighborList = new ArrayList<Vertex>();
            for (Path path : paths) {
                if (path.getStart().equals(point.getId())) {
                    neighborList.add(new Vertex(path.getEnd(), path.getLength()));
                } else if (path.getEnd().equals(point.getId())) {
                    neighborList.add(new Vertex(path.getStart(), path.getLength()));
                }
            }
            vertexMap.put(point.getId(), neighborList);
        }

        return vertexMap;
    }

    /**
     * @param start 起点id
     * @param end   终点id
     * @return 路径id
     */
    public List<String> getShortestPath(String start, String end) {
        //所有节点和起点的距离
        final Map<String, Double> distances = new HashMap<String, Double>();
        //上一个点
        final Map<String, Vertex> previous = new HashMap<String, Vertex>();
        //所有待处理节点，使用距离排序
        PriorityQueue<Vertex> nodes = new PriorityQueue<Vertex>();

        //找到起点，初始化nodes
        for (String id : this.vertices.keySet()) {
            if (id.equals(start)) {
                distances.put(id, 0d);
                nodes.add(new Vertex(id, 0));
            } else {
                distances.put(id, Double.MAX_VALUE);
                nodes.add(new Vertex(id, Double.MAX_VALUE));
            }
            previous.put(id, null);
        }

        while (!nodes.isEmpty()) {
            //取出待处理的点中距离最短的
            Vertex smallest = nodes.poll();
            //找到终点退出
            if (smallest.getId().equals(end)) {
                final List<String> path = new ArrayList<String>();
                while (previous.get(smallest.getId()) != null) {
                    path.add(smallest.getId());
                    smallest = previous.get(smallest.getId());
                }
                path.add(start);
                Collections.reverse(path);
                return path;
            }
            //如果找不到新的最近点则代表无路可走，退出
            if (distances.get(smallest.getId()) == Double.MAX_VALUE) {
                break;
            }
            //找到最近点的邻近点
            for (Vertex neighbor : vertices.get(smallest.getId())) {
                //计算邻近点和起点的距离
                double neighborDistance = distances.get(smallest.getId()) + neighbor.getDistance();
                //如果该距离比记录的距离短则记录
                if (neighborDistance < distances.get(neighbor.getId())) {
                    distances.put(neighbor.getId(), neighborDistance);
                    previous.put(neighbor.getId(), smallest);
                    //更新距离
                    for (Vertex n : nodes) {
                        if (n.getId().equals(neighbor.getId())) {
                            nodes.remove(n);
                            n.setDistance(neighborDistance);
                            nodes.add(n);
                            break;
                        }
                    }
                }
            }
        }
        return null;
    }

    private Double getOffsetWidth(String start, String end) throws Exception {
        if (this.offsetMap == null) {
            throw new Exception("未初始化");
        }
        String key = start + "_" + end;
        return this.offsetMap.getOrDefault(key, 0d);
    }

}
